/*
 *        (C) COPYRIGHT Ingenic Limited.
 *             ALL RIGHTS RESERVED
 *
 * File       : lib_carto_contour.h
 * Authors    : ycai@ubuntu
 * Create Time: 2022-06-09:01:45:38
 * Description:
 *
 */

#ifndef __LIB_CARTO_CONTOUR_H__
#define __LIB_CARTO_CONTOUR_H__
#include <iostream>
#include <fstream>
#include <sstream>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "lib_point_cloud_raw.h"



// Convert from map index to world coords
#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)

// Convert from world coords to map coords
#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)

// Test to see if the given map coords lie within the absolute map bounds.
#define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y))

// Compute the cell index for the given map coords.
#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)


// Description for a single map cell.
typedef struct
{
  // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
  int occ_state;

  // Distance to the nearest occupied cell
  double occ_dist;

  // Wifi levels
  //int wifi_levels[MAP_WIFI_MAX_LEVELS];

} map_cell_t;


// Description for a map
typedef struct
{
  // Map origin; the map is a viewport onto a conceptual larger map.
  double origin_x, origin_y;
  
  // Map scale (m/cell)
  double scale;

  // Map dimensions (number of cells)
  int size_x, size_y;
  
  // The map data, stored as a grid
  map_cell_t *cells;

  // Max distance at which we care about obstacles, for constructing
  // likelihood field
  double max_occ_dist;
  
} map_t;

class carto_map_type
{
 public:
  carto_map_type(const char* yaml_p,const char* pgm_p);
  ~carto_map_type();

  // 从指定的反光板地图csv文件中读取反光板坐标数据，存入结构体变量map中
  int map_load_occ(map_t *map, const char *filename, double scale, int negate);

  int map_load_yaml(const char *filename, double *scale, int *negate,double *origin_x, double *origin_y);
  
  map_t* map_;
   
 private:
  carto_map_type(const carto_map_type&);  // 禁用拷贝构造函数
  void operator=(const carto_map_type&);
};


//Extract a laser beam from the map 
double cartomap_calc_range(map_t *map, double ox, double oy, double oa, double max_range);

//Extract scan from the map
void extract_scan_from_cartomap(const carto_map_type* contour_map,point_cloud_raw_type* point_cloud_map_raw,const pose_type* laser_scanner_pose,double max_range);






#endif /* __LIB_CARTO_CONTOUR_H__ */

